{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "8c577d78-3e4a-4f08-93ed-5c60867b9a3b",
   "metadata": {
    "id": "hairy-humidity"
   },
   "source": [
    "<center>\n",
    "<h4>CDS 110, Lecture 7</h4>\n",
    "<font color=blue><h1>Frequency Domain Analysis using Bode/Nyquist plots</h1></font>\n",
    "<h3>Richard M. Murray, Winter 2024</h3>\n",
    "</center>\n",
    "\n",
    "[Open in Google Colab](https://colab.research.google.com/drive/1-BIaln1nF41fGqavzliuWT74nBkAnM3x)\n",
    "\n",
    "The purpose of this lecture is to introduce tools that can be used for frequency domain modeling and analysis of linear systems.  It illustrates the use of a variety of frequency domain analysis and plotting tools."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "invalid-carnival",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Import standard packages needed for this exercise\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import math\n",
    "try:\n",
    "  import control as ct\n",
    "  print(\"python-control\", ct.__version__)\n",
    "except ImportError:\n",
    "  !pip install control\n",
    "  import control as ct\n",
    "\n",
    "# Use ctrlplot defaults for matplotlib\n",
    "plt.rcParams.update(ct.rcParams)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "P7t3Nm4Tre2Z",
   "metadata": {
    "id": "P7t3Nm4Tre2Z"
   },
   "source": [
    "## Stable system: servomechanism\n",
    "\n",
    "We start with a simple example a stable system for which we wish to design a simple controller and analyze its performance, demonstrating along the way the basic frequency domain analysis functions in the Python control toolbox (python-control).\n",
    "\n",
    "Consider a simple mechanism for positioning a mechanical arm whose equations of motion are given by\n",
    "\n",
    "$$\n",
    "J \\ddot \\theta = -b \\dot\\theta - k r\\sin\\theta + \\tau_\\text{m},\n",
    "$$\n",
    "\n",
    "which can be written in state space form as\n",
    "\n",
    "$$\n",
    "\\frac{d}{dt} \\begin{bmatrix} \\theta \\\\ \\theta \\end{bmatrix} =\n",
    "  \\begin{bmatrix} \\dot\\theta \\\\ -k r \\sin\\theta / J - b\\dot\\theta / J \\end{bmatrix}\n",
    "  + \\begin{bmatrix} 0 \\\\ 1/J \\end{bmatrix} \\tau_\\text{m}.\n",
    "$$\n",
    "\n",
    "The system consists of a spring loaded arm that is driven by a  motor, as shown below.\n",
    "\n",
    "<center><img src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/servomech-diagram.png\" alt=\"servomech-diagram\" width=\"240\"></center>\n",
    "\n",
    "The motor applies a torque that twists the arm against a linear spring and moves the end of the arm across a rotating platter. The input to the system is the motor torque $\\tau_\\text{m}$. The force exerted by the spring is a nonlinear function of the head position due to the way it is attached.\n",
    "\n",
    "The system parameters are given by\n",
    "\n",
    "$$\n",
    "k = 1,\\quad J = 100,\\quad b = 10,\n",
    "\\quad r = 1,\\quad l = 2,\\quad \\epsilon = 0.01,\n",
    "$$\n",
    "\n",
    "and we assume that time is measured in msec and distance in cm.  (The constants here are made up and don't necessarily reflect a real disk drive, though the units and time constants are motivated by computer disk drives.)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3e476db9",
   "metadata": {
    "id": "3e476db9"
   },
   "source": [
    "The system dynamics can be modeled in python-control using a `NonlinearIOSystem` object, which we create with the `nlsys` function:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "27bb3c38",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Parameter values\n",
    "servomech_params = {\n",
    "    'J': 100,             # Moment of inertia of the motor\n",
    "    'b': 10,              # Angular damping of the arm\n",
    "    'k': 1,               # Spring constant\n",
    "    'r': 1,               # Location of spring contact on arm\n",
    "    'l': 2,               # Distance to the read head\n",
    "    'eps': 0.01,          # Magnitude of velocity-dependent perturbation\n",
    "}\n",
    "\n",
    "# State derivative\n",
    "def servomech_update(t, x, u, params):\n",
    "    # Extract the configuration and velocity variables from the state vector\n",
    "    theta = x[0]                # Angular position of the disk drive arm\n",
    "    thetadot = x[1]             # Angular velocity of the disk drive arm\n",
    "    tau = u[0]                  # Torque applied at the base of the arm\n",
    "\n",
    "    # Get the parameter values\n",
    "    J, b, k, r = map(params.get, ['J', 'b', 'k', 'r'])\n",
    "\n",
    "    # Compute the angular acceleration\n",
    "    dthetadot = 1/J * (\n",
    "        -b * thetadot - k * r * np.sin(theta) + tau)\n",
    "\n",
    "    # Return the state update law\n",
    "    return np.array([thetadot, dthetadot])\n",
    "\n",
    "# System output (end of arm)\n",
    "def servomech_output(t, x, u, params):\n",
    "    l = params['l']\n",
    "    return np.array([l * x[0]])\n",
    "\n",
    "# System dynamics\n",
    "servomech = ct.nlsys(\n",
    "    servomech_update, servomech_output, name='servomech',\n",
    "    params=servomech_params,\n",
    "    states=['theta_', 'thdot_'],\n",
    "    outputs=['y'], inputs=['tau'])\n",
    "\n",
    "print(servomech)\n",
    "print(\"\\nParams:\", servomech.params)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "competitive-terrain",
   "metadata": {
    "id": "competitive-terrain"
   },
   "source": [
    "### Linearization\n",
    "\n",
    "To study the open loop dynamics of the system, we compute the linearization of the dynamics about the equilibrium point corresponding to $\\theta_\\text{e} = 15^\\circ$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "senior-carpet",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Convert the equilibrium angle to radians\n",
    "theta_e = (15 / 180) * np.pi\n",
    "\n",
    "# Compute the input required to hold this position\n",
    "u_e = servomech.params['k'] * servomech.params['r'] * np.sin(theta_e)\n",
    "print(\"Equilibrium torque = %g\" % u_e)\n",
    "\n",
    "# Linearize the system about the equilibrium point\n",
    "P = servomech.linearize([theta_e, 0], u_e, name='P_ss')\n",
    "P.name = 'P_ss'  # TODO: fix in nlsys_improvements\n",
    "print(\"Linearized dynamics:\", P)\n",
    "print(\"Zeros: \", P.zeros())\n",
    "print(\"Poles: \", P.poles())\n",
    "print(\"\")\n",
    "\n",
    "# Transfer function representation\n",
    "P_tf = ct.tf(P, name='P_tf')\n",
    "print(P_tf)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "instant-lancaster",
   "metadata": {
    "id": "instant-lancaster"
   },
   "source": [
    "### Open loop frequency response\n",
    "\n",
    "A standard method for understanding the dynamics is to plot the output of the system in response to sinusoids with unit magnitude at different frequencies.\n",
    "\n",
    "We use the `frequency_response` function to plot the step response of the linearized, open-loop system."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "RxXFTpwO5bGI",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Reset the frequency response label to correspond to a time unit of ms\n",
    "ct.set_defaults('freqplot', freq_label=\"Frequency [rad/ms]\")\n",
    "\n",
    "# Frequency response\n",
    "freqresp = ct.frequency_response(P, np.logspace(-2, 0))\n",
    "freqresp.plot()\n",
    "\n",
    "# Equivalent command\n",
    "ct.bode_plot(P_tf, np.logspace(-2, 0), '--')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "stuffed-premiere",
   "metadata": {
    "id": "stuffed-premiere"
   },
   "source": [
    "### Feedback control design\n",
    "\n",
    "We next design a feedback controller for the system using a proportional integral controller, which has transfer function\n",
    "\n",
    "$$\n",
    "C(s) = \\frac{k_\\text{p} s + k_\\text{i}}{s}\n",
    "$$\n",
    "\n",
    "We will learn how to choose $k_\\text{p}$ and $k_\\text{i}$ more formally in W9.  For now we just pick different values to see how the dynamics are impacted."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8NK8O6XT7B_a",
   "metadata": {},
   "outputs": [],
   "source": [
    "kp = 1\n",
    "ki = 1\n",
    "\n",
    "# Create tf from numerator/denominator coefficients\n",
    "C = ct.tf([kp, ki], [1, 0], name='C')\n",
    "print(C)\n",
    "\n",
    "# Alternative method: define \"s\" and use algebra\n",
    "s = ct.tf('s')\n",
    "C = ct.tf(kp + ki/s, name='C')\n",
    "print(C)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "074427a3",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Loop transfer function\n",
    "L = P * C\n",
    "cplt = ct.bode_plot([P, C, L], label=['P', 'C', 'L'])\n",
    "cplt.set_plot_title(\"PI controller for servomechanism\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "Bg5ga11VuRtI",
   "metadata": {
    "id": "Bg5ga11VuRtI"
   },
   "source": [
    "Note that L = P * C corresponds to addition in both the magnitude and the phase."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "UmYmSzx2rTfg",
   "metadata": {
    "id": "UmYmSzx2rTfg"
   },
   "source": [
    "### Nyquist analysis\n",
    "\n",
    "To check stability (and eventually robustness), we use the Nyquist criterion."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "Qmp59pmS9GLj",
   "metadata": {},
   "outputs": [],
   "source": [
    "fig = plt.figure(figsize=[7, 4])\n",
    "ax1 = plt.subplot(2, 2, 1)\n",
    "ax2 = plt.subplot(2, 2, 3)\n",
    "ct.bode_plot(L, ax=[ax1, ax2])\n",
    "\n",
    "# Tidy up the figure a bit\n",
    "fig.align_labels()\n",
    "ax1.set_title(\"Bode plot for L\")\n",
    "\n",
    "ax2 = plt.subplot(1, 2, 2)\n",
    "ct.nyquist_plot(L, ax=ax2, title=\"\")\n",
    "plt.title(\"Nyquist plot for L\")\n",
    "\n",
    "plt.suptitle(\"Loop analysis for (unstable) servomechanism\")\n",
    "plt.tight_layout()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "s4dDf4PrZqU3",
   "metadata": {
    "id": "s4dDf4PrZqU3"
   },
   "source": [
    "We see from this plot that the loop transfer function encircles the -1 point => closed loop system should be unstable.  We can check this by making use of additional features of Nyquist analysis."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "K7ifUBL0Z3xN",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Get the Nyquist *response*, so that we can get back encirclements\n",
    "nyqresp = ct.nyquist_response(L)\n",
    "print(\"N = encirclements: \", nyqresp.count)\n",
    "print(\"P = RHP poles of L: \", np.sum(np.real(L.poles()) > 0))\n",
    "print(\"Z = N + P = RHP zeros of 1 + L:\", np.sum(np.real((1 + L).zeros()) > 0))\n",
    "print(\"Zeros of (1 + L) = \", (1 + L).zeros())\n",
    "print(\"\")\n",
    "\n",
    "T = ct.feedback(L)\n",
    "ct.step_response(T).plot(\n",
    "    title=\"Step response for (unstable) servomechanism\",\n",
    "    time_label=\"Time [ms]\");"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "p3JxLilMxdOE",
   "metadata": {
    "id": "p3JxLilMxdOE"
   },
   "source": [
    "### Poles on the $j\\omega$ axis\n",
    "\n",
    "Note that we have a pole at 0 (due to the integrator in the controller).  How is this handled?\n",
    "\n",
    "A: use a small loop to the right around poles on the $j\\omega$ axis => not inside the contour.\n",
    "\n",
    "To see this, we use the `nyquist_response` function, which returns the contour used to compute the Nyquist curve.  If we zoom in on the contour near the origin, we see how the outer edge of the Nyquist curve is computed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "R5IBk3Ai9Slk",
   "metadata": {},
   "outputs": [],
   "source": [
    "fig = plt.figure(figsize=[7, 5.8])\n",
    "\n",
    "# Plot the D contour\n",
    "ax1 = plt.subplot(2, 2, 1)\n",
    "plt.plot(np.real(nyqresp.contour), np.imag(nyqresp.contour))\n",
    "plt.axis([-1e-4, 4e-4, 0, 4e-4])\n",
    "plt.xlabel('Real axis')\n",
    "plt.ylabel('Imaginary axis')\n",
    "plt.title(\"Zoom on D-contour\")\n",
    "\n",
    "# Clean up the display of the units\n",
    "from matplotlib import ticker\n",
    "ax1.xaxis.set_major_formatter(ticker.StrMethodFormatter(\"{x:.0e}\"))\n",
    "ax1.yaxis.set_major_formatter(ticker.StrMethodFormatter(\"{x:.0e}\"))\n",
    "\n",
    "ax2 = plt.subplot(2, 2, 2)\n",
    "ct.nyquist_plot(L, ax=ax2)\n",
    "plt.title(\"Nyquist curve\")\n",
    "\n",
    "plt.suptitle(\"Nyquist contour for pole at the origin\")\n",
    "plt.tight_layout()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "h20JRZ_r4fGy",
   "metadata": {
    "id": "h20JRZ_r4fGy"
   },
   "source": [
    "### Second iteration feedback control design\n",
    "\n",
    "We now redesign the control system to give something that is stable.  We can do this by moving the zero for the controller to a lower frequency, so that the phase lag from the integrator does not overlap with the phase lag from the system dynamics."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "YsM8SnXz_Kaj",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Change the frequency response to avoid crossing over -180 with large gain\n",
    "Cnew = ct.tf(kp + (ki/200)/s, name='C_new')\n",
    "Lnew = ct.tf(P * Cnew, name='L_new')\n",
    "\n",
    "plt.figure(figsize=[7, 4])\n",
    "ax1 = plt.subplot(2, 2, 1)\n",
    "ax2 = plt.subplot(2, 2, 3)\n",
    "ct.bode_plot([Lnew, L], ax=[ax1, ax2], label=['L_new', 'L_old'])\n",
    "\n",
    "# Clean up the figure a bit\n",
    "ax1.loglog([1e-3, 1e1], [1, 1], 'k', linewidth=0.5)\n",
    "ax1.set_title(\"Bode plot for L_new, L_old\", size='medium')\n",
    "\n",
    "ax3=plt.subplot(1, 2, 2)\n",
    "ct.nyquist_plot(Lnew, max_curve_magnitude=5, ax=ax3)\n",
    "ax3.set_title(\"Nyquist plot for Lnew\", size='medium')\n",
    "\n",
    "plt.suptitle(\"Loop analysis for (stable) servomechanism\")\n",
    "plt.tight_layout()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "kFjeGXzDvucx",
   "metadata": {
    "id": "kFjeGXzDvucx"
   },
   "source": [
    "We see now that we have no encirclements, and so the system should be stable.\n",
    "\n",
    "Note however that the Nyquist curve is close to the -1 point => not *that* stable."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "GGfJwG716jU2",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compute the transfer function from r to y\n",
    "Tnew = ct.feedback(Lnew)\n",
    "cplt = ct.step_response(Tnew).plot(time_label=\"Time [ms]\")\n",
    "cplt.set_plot_title(\"Step response for (stable) spring-mass system\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b5114fa7-6924-47d7-8dd2-f12060152edd",
   "metadata": {},
   "source": [
    "### Third iteration feedback control design (via loop shaping)\n",
    "\n",
    "To get a better design, we use a PID controller to shape the frequency response so that we get high gain at low frequency and low phase at crossover."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e6da93a4-5202-45d7-9e5a-697848f4ba71",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Design parameters\n",
    "Td = 1                    # Set to gain crossover frequency\n",
    "Ti = Td * 10              # Set to low frequency region\n",
    "kp = 500                  # Tune to get desired bandwith\n",
    "\n",
    "# Updated gains\n",
    "kp = 150\n",
    "Ti = Td * 5; kp = 150\n",
    "\n",
    "# Compute controller parmeters\n",
    "ki = kp/Ti\n",
    "kd = kp * Td\n",
    "\n",
    "# Controller transfer function\n",
    "ctrl_shape = kp + ki / s + kd * s\n",
    "\n",
    "# Frequency response (open loop) - use this to help tune your design\n",
    "ltf_shape = ct.tf(P_tf * ctrl_shape, name='L_shape')\n",
    "\n",
    "cplt = ct.frequency_response([P, ctrl_shape]).plot(label=['P', 'C_shape'])\n",
    "cplt = ct.frequency_response(ltf_shape).plot(margins=True)\n",
    "\n",
    "cplt.set_plot_title(\"Loop shaping design for servomechanism controller\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d731f372-4992-464c-9ca5-49cc1d554799",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compute the transfer function from r to y\n",
    "T_shape = ct.feedback(ltf_shape)\n",
    "cplt = ct.step_response(T_shape).plot(\n",
    "    time_label=\"Time [ms]\",\n",
    "    title = \"Step response for servomechanism with PID controller\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "JL99vo4trep5",
   "metadata": {
    "id": "JL99vo4trep5"
   },
   "source": [
    "### Closed loop frequency response\n",
    "\n",
    "We can also look at the closed loop frequency response to understand how different inputs affect different outputs.  The `gangof4` function computes the standard transfer functions:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ceqcg3oM619g",
   "metadata": {},
   "outputs": [],
   "source": [
    "cplt = ct.gangof4(P_tf, ctrl_shape)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "gel18-iqwYYs",
   "metadata": {
    "id": "gel18-iqwYYs"
   },
   "source": [
    "### Stability margins\n",
    "\n",
    "Another standard set of analysis tools is to identify the gain, phase, and stability margins for the system:\n",
    "\n",
    "* **Gain margin:** the maximimum amount of additional gain that we can put into the loop and still maintain stability.\n",
    "* **Phase margin:** the maximum amount of additional phase (lag) that we can put into the loop and still maintain stability.\n",
    "* **Stability margin:** the maximum amount of combined gain and phase at the critical frequency that can be put into the loop and still maintain stability.\n",
    "\n",
    "The first two of the items can be computed either by looking at the frequency response or by using the `margin` command.\n",
    "\n",
    "The stabilty margin is the minimum distance between -1 and $L(jw)$, which is just the minimum value of $|1 - L(j\\omega)|$.\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "m-8ItbHwxLrv",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.figure(figsize=[7, 4])\n",
    "\n",
    "# Gain and phase margin on Bode plot\n",
    "ax1 = plt.subplot(2, 2, 1)\n",
    "plt.title(\"Bode plot for Lnew, with margins\")\n",
    "ax2 = plt.subplot(2, 2, 3)\n",
    "ct.bode_plot(Lnew, ax=[ax1, ax2], margins=True)\n",
    "\n",
    "# Compute gain and phase margin\n",
    "gm, pm, wpc, wgc = ct.margin(Lnew)\n",
    "print(f\"Gm = {gm:2.2g} (at {wpc:.2g} rad/ms)\")\n",
    "print(f\"Pm = {pm:3.2g} deg (at {wgc:.2g} rad/ms)\")\n",
    "\n",
    "# Compute the stability margin\n",
    "resp = ct.frequency_response(1 + Lnew)\n",
    "sm = np.min(resp.magnitude)\n",
    "wsm = resp.omega[np.argmin(resp.magnitude)]\n",
    "print(f\"Sm = {sm:2.2g} (at {wsm:.2g} rad/ms)\")\n",
    "\n",
    "# Plot the Nyquist curve\n",
    "ax3 = plt.subplot(1, 2, 2)\n",
    "ct.nyquist_plot(Lnew, ax=ax3)\n",
    "plt.title(\"Nyquist plot for Lnew [zoomed]\")\n",
    "plt.axis([-2, 3, -2.6, 2.6])\n",
    "\n",
    "#\n",
    "# Annotate it to see the margins\n",
    "#\n",
    "\n",
    "# Gain margin (special case here, since infinite)\n",
    "Lgm = 0\n",
    "plt.plot([-1, Lgm], [0, 0], 'k-', linewidth=0.5)\n",
    "plt.text(-0.9, 0.1, \"1/gm\")\n",
    "\n",
    "# Phase margin\n",
    "theta = np.linspace(0, 2 * math.pi)\n",
    "plt.plot(np.cos(theta), np.sin(theta), 'k--', linewidth=0.5)\n",
    "plt.text(-1.3, -0.8, \"pm\")\n",
    "\n",
    "# Stability margin\n",
    "Lsm = Lnew(wsm * 1j)\n",
    "plt.plot([-1, Lsm.real], [0, Lsm.imag], 'k-', linewidth=0.5)\n",
    "plt.text(-0.4, -0.5, \"sm\")\n",
    "\n",
    "plt.suptitle(\"\")\n",
    "plt.tight_layout()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "WsOzQST9rFC-",
   "metadata": {
    "id": "WsOzQST9rFC-"
   },
   "source": [
    "## Unstable system: inverted pendulum\n",
    "\n",
    "When we have a system that is open loop unstable, the Nyquist curve will need to have encirclements to be stable.  In this case, the interpretation of the various characteristics can be more complicated.\n",
    "\n",
    "To explore this, we consider a simple model for an inverted pendulum, which has (normalized) dynamics:\n",
    "\n",
    "$$\n",
    "\\dot x = \\begin{bmatrix} 0 & 1 & \\\\ -1 & 0.1 \\end{bmatrix} x + \\begin{bmatrix} 0 \\\\ 1 \\end{bmatrix} u, \\qquad\n",
    "y = \\begin{bmatrix} 1 & 0 \\end{bmatrix} x\n",
    "$$\n",
    "\n",
    "Transfer function for the system can be shown to be\n",
    "\n",
    "$$\n",
    "P(s) = \\frac{1}{s^2 + 0.1 s - 1}.\n",
    "$$\n",
    "\n",
    "This system is unstable, with poles $\\sim\\pm 1$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ZbPzrlPIrHnp",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = ct.tf([1], [1, 0.1, -1])\n",
    "P.poles()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "W-sBWxKi6SPx",
   "metadata": {
    "id": "W-sBWxKi6SPx"
   },
   "source": [
    "### PD controller\n",
    "\n",
    "We construct a proportional-derivative (PD) controller for the system,\n",
    "\n",
    "$$\n",
    "u = k_\\text{p} e + k_\\text{d} \\dot{e}\n",
    "$$\n",
    "\n",
    "which is roughly the equivalent of using state feedback (since the system states are $\\theta$ and $\\dot\\theta$)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "hjQS_dED7yJE",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Transfer function for a PD controller\n",
    "kp = 10\n",
    "kd = 2\n",
    "C = ct.tf([kd, kp], [1])\n",
    "\n",
    "# Loop transfer function\n",
    "L = P * C\n",
    "L.name = 'L'\n",
    "print(L)\n",
    "print(\"Zeros: \", L.zeros())\n",
    "print(\"Poles: \", L.poles())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "YI_KJo0E9pFd",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Bode and Nyquist plots\n",
    "plt.figure(figsize=[7, 4])\n",
    "ax1 = plt.subplot(2, 2, 1)\n",
    "plt.title(\"Bode plot for L\", size='medium')\n",
    "ax2 = plt.subplot(2, 2, 3)\n",
    "ct.bode_plot(L, ax=[ax1, ax2])\n",
    "\n",
    "ax3 = plt.subplot(1, 2, 2)\n",
    "ct.nyquist_plot(L, ax=ax3)\n",
    "plt.title(\"Nyquist plot for L\", size='medium')\n",
    "\n",
    "plt.suptitle(\"Loop analysis for inverted pendulum\")\n",
    "plt.tight_layout()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8dH03kv9-Da8",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Check the Nyquist criterion\n",
    "nyqresp = ct.nyquist_response(L)\n",
    "print(\"N = encirclements: \", nyqresp.count)\n",
    "print(\"P = RHP poles of L: \", np.sum(np.real(L.poles()) > 0))\n",
    "print(\"Z = N + P = RHP zeros of 1 + L:\", np.sum(np.real((1 + L).zeros()) >= 0))\n",
    "print(\"Poles of L = \", L.poles())\n",
    "print(\"Zeros of 1 + L = \", (1 + L).zeros())\n",
    "print(\"\")\n",
    "\n",
    "T = ct.feedback(L)\n",
    "ct.initial_response(T, X0=[0.1, 0]).plot();"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7bb03f68-0c99-40e9-86cd-a9f2816b4096",
   "metadata": {},
   "source": [
    "Note that we get a warning when we set the initial condition.  This is because `T` is a transfer function and so it doesn't have a unique state space realization.  If the initial state is zero this doesn't matter, but if the initial state is nonzero then the assignment of states is not well defined."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "VXlYhs8X7DuN",
   "metadata": {
    "id": "VXlYhs8X7DuN"
   },
   "source": [
    "### Gang of 4\n",
    "\n",
    "Another useful thing to look at is the transfer functions from noise and disturbances to the system outputs and inputs:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "oTmOun41_opt",
   "metadata": {},
   "outputs": [],
   "source": [
    "ct.gangof4(P, C);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "U41ve1zh7XPh",
   "metadata": {
    "id": "U41ve1zh7XPh"
   },
   "source": [
    "We see that the response from the input $r$ (or equivalently noise $n$) to the process input is very large for large frequencies.  This means that we are amplifying high frequency noise (and comes from the fact that we used derivative feedback)."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "YROqmZTd8WYs",
   "metadata": {
    "id": "YROqmZTd8WYs"
   },
   "source": [
    "### High frequency rolloff\n",
    "\n",
    "We can attempt to resolve this by \"rolling off\" the derivative action at high frequencies:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "vhKi_L-F_6Ws",
   "metadata": {},
   "outputs": [],
   "source": [
    "Cnew = (kp + kd * s) / (s/20 + 1)**2\n",
    "Cnew.name = 'Cnew'\n",
    "print(Cnew)\n",
    "\n",
    "Lnew = P * Cnew\n",
    "Lnew.name = 'Lnew'\n",
    "\n",
    "plt.figure(figsize=[7, 4])\n",
    "ax1 = plt.subplot(2, 2, 1)\n",
    "ax2 = plt.subplot(2, 2, 3)\n",
    "ct.bode_plot([Lnew, L], ax=[ax1, ax2])\n",
    "ax1.loglog([1e-1, 1e2], [1, 1], 'k', linewidth=0.5)\n",
    "ax1.set_title(\"Bode plot for L, Lnew\", size='medium')\n",
    "\n",
    "ax3 = plt.subplot(1, 2, 2)\n",
    "ct.nyquist_plot(Lnew, ax=ax3)\n",
    "ax3.set_title(\"Nyquist plot for Lnew\", size='medium')\n",
    "\n",
    "plt.suptitle(\"Stability analysis for inverted pendulum\")\n",
    "plt.tight_layout()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "WgrAE9XE7_nJ",
   "metadata": {
    "id": "WgrAE9XE7_nJ"
   },
   "source": [
    "While not (yet) a very high performing controller, this change does get rid of the issues with the high frequency noise:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "FknwW6GkBLLU",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Check the gang of 4\n",
    "ct.gangof4(P, Cnew);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "wJHJLjXwCNz-",
   "metadata": {},
   "outputs": [],
   "source": [
    "# See what the step response looks like\n",
    "Tnew = ct.feedback(Lnew)\n",
    "ct.step_response(Tnew, 10).plot()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "WUhz529a-w3q",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
